# -*- coding: utf-8 -*-
"""
Created on Mon Jul  6 00:47:01 2020

@author: Erfan
"""
from __future__ import print_function, division
import time
import numpy as np
import matplotlib.pyplot as plt
from casadi import *
import random
from Model import *
from Parameters import *
from Extended_Kalman_Filter import *
from read_data import get_data
import csv
import pandas as pd


Nr,Nt,Nz,dr,dt,dz,Np,NN,Nx,Nw,Ny,Nv=circular_parameters()
DeltaT,Nsim=time_parameters()

np.random.seed(78)
x0=np.random.uniform(-4,-3,Nx)
x0_est=1*x0

    #Normalization
delta_value = 33.1045
min_value = -33.1619
x0_est_scaled = (x0_est - min_value)/delta_value


            
N_measurements = 2
Sigma_p=12
Sigma_w=0.005
Sigma_v=1
P_pre=np.diag(Sigma_p*np.ones((Nx,)))
Q=np.diag(Sigma_w*np.ones((Nx,)))
R=np.diag(Sigma_v*np.ones((N_measurements,)))


uu=np.zeros(Nsim)
uu[1724:1764]=-2.20485e-06 #4 July
uu[3444:3455]=-2.20485e-06 #18 July
uu[3644:3684]=-2.20485e-06 #20 July
uu[4404:4415]=-2.20485e-06 #26 July
uu[4884:4895]=-2.20485e-06 #30 July
uu[5724:5764]=-2.20485e-06 #6 Aug, 7Aug (continuous)

seedpoint=500
np.random.seed(seedpoint)
ww=0*np.random.randn(Nsim,Nw) ##Process Noise
ww_1=0*np.random.randn(Nsim,Nw) 
vv=0*np.random.randn(Nsim+1,Nv) ## Measurement Noise

#Kc=0.75*np.ones(Nsim)
Kc=np.zeros(Nsim)
Et=np.zeros(Nsim)
#Et=3.94485e-09*np.ones(Nsim)

xx_ekf = np.zeros((Nsim+1,Nx))        
xx_ekf[0, :] = x0_est_scaled

## Creating the Casadi Integrator
x_symbol = SX.sym('X',Nx) # The states
u_symbol=SX.sym('U',1)# The input which is the irrigation amount
w_symbol=SX.sym('W',Nw)# The disturbance
i_symbol=SX.sym('i_symbol',1)
Kc_symbol=SX.sym('Kc_symbol',1)
Et0_symbol=SX.sym('Et0_symbol',1)

F_symbol = ode_CP(x_symbol,u_symbol,w_symbol,i_symbol,Kc_symbol,Et0_symbol)
ODE = {'x': x_symbol, 'p': vertcat(u_symbol,w_symbol,i_symbol,Kc_symbol,Et0_symbol), 'ode': F_symbol}
opts = {'tf': DeltaT, 'regularity_check':True}  
I = integrator('I', 'cvodes', ODE, opts)  # Build casadi integrator

for i in range(1, Nsim+1):
    e1=time.time()
    print(i)
    d=i
    
        ## Un normalization
    xx_ekf_unscaled = ( xx_ekf[i-1] * delta_value ) + min_value
    
    Ik1 = I(x0=xx_ekf_unscaled, p=vertcat(uu[i-1],ww[i-1],d,Kc[i-1],Et[i-1]))      # EKF Simulation
    xk1 = Ik1['xf'].full().ravel()

        #Normalization
    xk1_scaled = (xk1 - min_value)/delta_value
    xx_ekf[i,:]=xk1_scaled
    
    if (i)%5==0:
        x_measurement =  get_data(int(i/5))
        print(x_measurement)
        C =  np.zeros((N_measurements,Nx))    
        C[0,3424]=1 
        C[1,1862]=1 
        
        x_cur1=xx_ekf[i,:]
        x_pre1=xx_ekf[i-1,:]
        u_pre=uu[i-1]
        
        
        [x_ekf_cur,P_plus_cur]=discrete_EKF(x_pre1,x_cur1,u_pre,x_measurement,P_pre,Q,R,C,ww_1[i-1],d,Kc[i-1],Et[i-1])
        
        xx_ekf[i,:]=x_ekf_cur
        P_pre1=P_plus_cur

    e2=time.time()-e1
    print('each step takes',e2,'seconds to solve')
 
    

    ## Un normalization and Save
xx_ekf_unscaled = ( xx_ekf * delta_value ) + min_value
np.savetxt('xx_ekf_unscaled.txt',xx_ekf_unscaled)


